﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  class BayesSelfLocator : SelfLocator
  {
    Type currentWorldStateType = null;
    BeliefProbabilityDistribution belief;
    int currentIndex = 0;

    public BayesSelfLocator()
    {
      belief = new BeliefProbabilityDistribution();
    }

    void InitBelief(Type newWorldStateType)
    {
      currentWorldStateType = newWorldStateType;
      belief.StatesWithProbability.Clear();
      if (currentWorldStateType == typeof(WorldState1D))
      {
        for (int i = Globals.stateSpaceMinimumX; i <= Globals.stateSpaceMaximumX; i += Globals.rasterWidth)
        {
          belief.AddValue(new WorldState1D(i), 0.5);
        }
        belief.Normalize();
      }

      if (currentWorldStateType == typeof(WorldState2D))
      {
        for (int x = Globals.stateSpaceMinimumX; x <= Globals.stateSpaceMaximumX; x += Globals.rasterWidth)
        {
          for (int y = Globals.stateSpaceMinimumY; y <= Globals.stateSpaceMaximumY; y += Globals.rasterWidth)
          {
            belief.AddValue(new WorldState2D(x, y), 0.5);
          }
        }
        belief.Normalize();
      }
    }

    public override WorldState Execute(Experiment experiment)
    {
      if(DebugKeyTool.IsActive("BayesSelfLocator:reset belief")) belief.InitWithEqualDistribution();

      if (experiment.actionAndObservationHistory.history.Count < currentIndex)
      {
        currentIndex = 0;
        belief.InitWithEqualDistribution();
      }

      if (experiment.simulatedWorldState.GetType() != currentWorldStateType)
        InitBelief(experiment.simulatedWorldState.GetType());
      //BAYES ALGORITHM
      while(currentIndex < experiment.actionAndObservationHistory.history.Count)
      {
        if (experiment.actionAndObservationHistory.history[currentIndex] is ActionDescription)
        {
          ActionDescription currentAction = experiment.actionAndObservationHistory.history[currentIndex] as ActionDescription;
//          belief = experiment.motionModel.UpdateBelief(belief, currentAction);
          belief.UpdateBeliefWithAction(experiment.motionModel, currentAction);
        }
        if (experiment.actionAndObservationHistory.history[currentIndex] is Measurement)
        {
          Measurement currentMeasurement = experiment.actionAndObservationHistory.history[currentIndex] as Measurement;
          belief = currentMeasurement.sensor.UpdateBelief(belief, experiment.map, currentMeasurement);
          belief.Normalize();
        }
        currentIndex++;
      }
      /*
      TimeTool.Start("durationOfMotionUpdate");
      TimeTool.Stop("durationOfMotionUpdate");
      TimeTool.Start("durationOfSensorUpdate");
      TimeTool.Stop("durationOfSensorUpdate");*/
      belief.Draw("world:BayesSelfLocator:probability");
      return belief.GetMaximum();
    }
  }
}
